import threading
import time

from robomaster import robot
import keyboard
from util import *

step = 50

robot.config.LOCAL_IP_STR = "192.168.10.2"

tl_drone = robot.Drone()
tl_drone.initialize()

log('connection succeeded')

tl_flight = tl_drone.flight

drone_version = tl_drone.get_sdk_version()
log("Drone sdk version: {0}".format(drone_version))

SN = tl_drone.get_sn()
log("drone sn: {0}".format(SN))

"""
控制飞机遥控器的四个杆量
参数:	
a – float:[-100, 100] 横滚
b – float:[-100, 100] 俯仰
c – float:[-100, 100] 油门
d – float:[-100, 100] 偏航
"""

received_order = ''
last_change_time = time.time()
have_stop = True


# 起飞
@fn_timer
def takeoff():
    tl_flight.takeoff().wait_for_completed()


# 左移
@fn_timer
def move_right():
    tl_flight.rc(a=step, b=0, c=0, d=0)


# 右移
@fn_timer
def move_left():
    tl_flight.rc(a=-step, b=0, c=0, d=0)


# 停止
@fn_timer
def stop():
    tl_flight.rc(a=0, b=0, c=0, d=0)


# 降落
@fn_timer
def land():
    tl_flight.land().wait_for_completed()


# 左旋
@fn_timer
def rotate_left():
    tl_flight.rc(a=0, b=0, c=0, d=-step)


# 右旋
@fn_timer
def rotate_right():
    tl_flight.rc(a=0, b=0, c=0, d=step)


# 左翻滚
@fn_timer
def roll_left():
    tl_flight.roll(-step).wait_for_completed()


# 右翻滚
@fn_timer
def roll_right():
    tl_flight.roll(step).wait_for_completed()


# 前进
@fn_timer
def move_forward():
    tl_flight.rc(a=0, b=step, c=0, d=0)


# 后退
@fn_timer
def move_backward():
    tl_flight.rc(a=0, b=-step, c=0, d=0)


@fn_timer
def move_up():
    tl_flight.rc(a=0, b=0, c=step, d=0)


@fn_timer
def move_down():
    tl_flight.rc(a=0, b=0, c=-step, d=0)


def set_receive_order(key):
    global received_order, last_change_time
    received_order = key
    last_change_time = time.time()


def callback(x):
    global received_order
    if x.event_type == 'down':
        if x.name == 'w':
            set_receive_order('w')
        elif x.name == 's':
            set_receive_order('s')
        elif x.name == 'd':
            set_receive_order('d')
        elif x.name == 'a':
            set_receive_order('a')

        elif x.name == '4':
            set_receive_order('4')
        elif x.name == '6':
            set_receive_order('6')
        elif x.name == '8':
            set_receive_order('8')
        elif x.name == '5':
            set_receive_order('5')

        elif x.name == 't':
            set_receive_order('t')
        elif x.name == 'l':
            set_receive_order('l')

        # elif x.name == 'q':
        #     set_receive_order('q')
        # elif x.name == 'e':
        #     set_receive_order('e')

        elif x.name == 'x':
            set_receive_order('x')
        elif x.name == 'esc':
            set_receive_order('esc')
            raise KeyboardInterrupt


def event_loop():
    global received_order, last_change_time, have_stop
    while True:
        now = time.time()
        if now - last_change_time > 0.25 and not have_stop:
            stop()
            have_stop = True
            last_change_time = now

        if received_order:
            have_stop = False
            if received_order == 'w':
                move_forward()
            elif received_order == 's':
                move_backward()
            elif received_order == 'a':
                move_left()
            elif received_order == 'd':
                move_right()

            elif received_order == '4':
                rotate_left()
            elif received_order == '6':
                rotate_right()
            elif received_order == '8':
                move_up()
            elif received_order == '5':
                move_down()

            elif received_order == 't':
                takeoff()
            elif received_order == 'l':
                land()

            # elif received_order == 'q':
            #     roll_left()
            # elif received_order == 'e':
            #     roll_right()

            elif received_order == 'x':
                stop()
            elif received_order == 'esc':
                print('esc')
                tl_drone.close()
                return
            received_order = ''
        time.sleep(0.01)


if __name__ == '__main__':
    try:
        t = threading.Thread(target=event_loop)
        t.setDaemon(True)
        t.start()
        keyboard.on_press(callback)
        keyboard.wait()
    except KeyboardInterrupt:
        log('exiting')
